#include "imu.h"
#include "bsp_uart.h"

#define IMU_SEND_SIZE 5U
uint8_t reset_z_mode1[IMU_SEND_SIZE] = {0xff, 0xaa, 0x69, 0x88, 0xb5};
uint8_t reset_z_mode2[IMU_SEND_SIZE] = {0xff ,0xaa, 0x01, 0x08, 0x00};
uint8_t reset_z_mode3[IMU_SEND_SIZE] = {0xff ,0xaa, 0x00, 0x00, 0x00};

static uint8_t imu_check_sum(uint8_t *p, uint8_t len);

static uint8_t imu_buff[IMU_BUFFER_LEN];

imu_t imu = {
	.init_state = 0,
};

/**
 * 初始化IMU
 */
void imu_init(void)
{
	ring_buf_init(&(imu.imu_rb), imu_buff, IMU_BUFFER_LEN);
}

/**
 * 按照通信时序,发送数据,初始化Z轴角度
 */
static void imu_reset_z(void)
{
	static uint8_t reset_z_mode = 0;
	static uint32_t times = 0;

	{
		switch (reset_z_mode)
		{
			case 0:
				reset_z_mode = 1;
				times = HAL_GetTick();
				uart4_send_bytes(reset_z_mode1, IMU_SEND_SIZE);
			break;
			case 1:
				if((HAL_GetTick() - times) >= 200)
				{
					reset_z_mode = 2;
					times = HAL_GetTick();
					uart4_send_bytes(reset_z_mode2, IMU_SEND_SIZE);
				}
			break;
			case 2:
				if((HAL_GetTick() - times) >= 3000)
				{
					reset_z_mode = 0;
					times = 0;
					uart4_send_bytes(reset_z_mode3, IMU_SEND_SIZE);
					imu.init_state = 1; /* IMU置位Z轴,完成 */
				}
			break;
		}
	}
}

/**
 * 校验和
 */
static uint8_t imu_check_sum(uint8_t *p, uint8_t len)
{
	uint8_t sum = 0;

	sum = 0x55 + 0x53; //imu数据帧头,为方便加在这里

	for(uint8_t i = 0;i < len;i++)
	{
		sum = sum + p[i];
	}

	return sum;
}

/**
*   按照单个字节解析接收数据
*/
static void imu_receive_analysis(uint8_t byte)
{
	//角度帧的头
    #define FRAME_HEADER_1 0x55
    #define FRAME_HEADER_2 0x53

    static uint8_t number = 0;
    static uint8_t pos = 0;
    uint8_t cal_crc = 0;

    LAB_ANALYSIS:
    switch (number)
    {
        case 0:  /* 找帧头1 */
        if(FRAME_HEADER_1 == byte)
        {
            number = 1;
        }
        break;
        case 1: /* 找帧头2 */
        if(FRAME_HEADER_2 == byte)
        {
            number = 2;
            pos = 0;
        }
        else
        {
            number = 0;
            goto LAB_ANALYSIS;
        }
        break;
        case 2: /* 获取数据 */
            imu.buff[pos++] = byte;
            if(pos >= 9)
            {
            	number = 3;
            }
        break;
        case 3: /* 解析数据 */
        	cal_crc = imu_check_sum(imu.buff, pos);
        	pos = 0;
        	if(cal_crc == imu.buff[8]) //第8字节是校验
        	{
        		imu.roll_x = (float)(((imu.buff[1]<<8)|imu.buff[0])/32768.0f*180.0f);
        		imu.pitch_y = (float)(((imu.buff[3]<<8)|imu.buff[2])/32768.0f*180.0f);
        		imu.yaw_z = (float)(((imu.buff[5]<<8)|imu.buff[4])/32768.0f*180.0f);

        		printf("yaw_z = %f .\r\n", imu.yaw_z);
        	}
        break;
    default:
        break;
    }
}

/* 接收数据处理 */
void imu_recv_handle(void)
{
	  if(imu.init_state == 0)//初始化z轴角度
	  {
		  imu_reset_z();
	  }
	  else
	  {
			uint16_t len = ring_buf_len(&imu.imu_rb);
			// 队列长度超限制
			if(len > IMU_BUFFER_LEN)
			{
				ring_buf_clr(&imu.imu_rb);
				return;
			}
			// 队列中有 len个数据
			if(len != 0)
			{
				for(uint8_t i = 0; i < len; i++)
				{
					uint8_t byte = 0;
					while (imu.imu_rb.lock == 1);
					ring_buf_get(&imu.imu_rb, &byte, 1);
					imu_receive_analysis(byte);
				}
			}
	  }
}

